package Code;

public class Obstacle {

	private Point[] corners;
	
	public Obstacle(String[] attrs) {
		corners = new Point[4];
		corners[0] = new Point(Double.parseDouble(attrs[1]), Double.parseDouble(attrs[2]));
		corners[1] = new Point(Double.parseDouble(attrs[3]), Double.parseDouble(attrs[4]));
		corners[2] = new Point(Double.parseDouble(attrs[5]), Double.parseDouble(attrs[6]));
		corners[3] = new Point(Double.parseDouble(attrs[7]), Double.parseDouble(attrs[8]));
	}
	
	public Obstacle(final Point point) {
		corners = new Point[4];
		for(int i = 0; i < 4; i++) {
			corners[i] = point;
		}
	}
	
	public Point getCorner(Corner corner) {
		return corners[corner.getPosition()];
	}
	
	public Point getCorner(int corner) {
		if(corner < 0 || corner >= 4) throw new IllegalArgumentException("must be between in [0, 4)");
		return corners[corner];
	}
	

	/**********************************************
	 * Calculate the closest point of 
	 * 		an obstacle to a given pt
	 * @param currPt the pt
	 * @return the closest pt
	 **********************************************/
	public Point calcClosesPt(Point currPt) 
	{
		Point perpPt;
		double minDist = Double.POSITIVE_INFINITY;
		Point minPoint = currPt;
		Point obstPt1;
		Point obstPt2;
		
		Point tempPt;
		double tempDist;
		
		for (int i = 0; i < corners.length; i++)
		{
			obstPt1 = corners[i];
			obstPt2 = corners[(i + 1) % corners.length];
			perpPt = calcPerpendicularPt(currPt, obstPt1, obstPt2);
			
			tempPt = determineOnSegment(perpPt, obstPt1, obstPt2);
			tempDist = currPt.calcDistance(tempPt);
			
			if (tempDist < minDist)
			{
				minDist = tempDist;
				minPoint = tempPt;
			}
			
			if (minDist == 0)	//on the obstacle
				break;
		}
		
		return minPoint;
	}

	private Point calcPerpendicularPt(Point currPt, Point obstPt1, Point obstPt2) 
	{
		double m1 = obstPt1.calcSlope(obstPt2);
		double x;
		double y;
		
		if (m1 == 0)	//horizontal line between obst corners
		{
			x = currPt.getX();
			y = obstPt1.getY();
		}
		else if (m1 == Double.POSITIVE_INFINITY) //vertical line between obst corners
		{
			x = obstPt2.getX();
			y = currPt.getY();
		}
		else
		{
			double m2 = 1d/m1;
			double b1 = obstPt1.calcYInt(obstPt2);
			double b2 = currPt.calcYInt(m2);
			x = (b2-b1)/(m1-m2);
			y = m1*x + b1;
		}
		return new Point(x, y);
	}

	/****************************************
	 * Determines whether a pt lies on a line segment
	 * 	if it doesn't then it finds the closest line on
	 * 	the line segment
	 * @param point		pt to be checked
	 * @param obstPt1	one endpt of line segment
	 * @param obstPt2	second endpt of line segment
	 * @return			closest pt on the line segment
	 */
	private Point determineOnSegment(Point point, Point obstPt1, Point obstPt2) 
	{
		Point closest = point;
		if (point.calcDistance(obstPt1) < point.calcDistance(obstPt2))
		{
			closest = obstPt1;
		}
		else
			closest = obstPt2;
		
		//check if within the x values
		if (obstPt1.getX() < obstPt2.getX())
		{
			if (point.getX() < obstPt1.getX() || point.getX() > obstPt2.getX())
				return closest;
		}
		else
		{
			if (point.getX() > obstPt1.getX() || point.getX() < obstPt2.getX())
				return closest;
		}
		
		//check if within y values
		if (obstPt1.getY() < obstPt2.getY())
		{
			if (point.getY() < obstPt1.getY() || point.getY() > obstPt2.getY())
				return closest;
		}
		else
		{
			if (point.getY() > obstPt1.getY() || point.getY() < obstPt2.getY())
				return closest;
		}
		
		return point;
	}
}
